#include "config.h"
/*
#include "prospeckz.h"
#include "speckmac.h"
#include <m8c.h>        // part specific constants and macros
#include "PSoCAPI.h"    // PSoC API definitions for all User Modules
#include "PSoCGPIOInt.h"
#include "stdlib.h"
#include "math.h"
#include "TxTimer.h"
#include "RxTimer.h"
#include "dsdv.h"
*/

//Internal defines
#define DC_RX_ALWAYS 8
#define DC_RX_BLOCKTX 4
#define DC_RX_BACKOFF 2
#define DC_RX_DATA	1
#define DC_RX_OFF	0


#define LoadRxTimer(timing) RxTimer_Stop();RxTimer_WritePeriod(timing);ClearInterrupt(RxTimer_INT_CLR, RxTimer_INT_MASK);RxTimer_Start();  
#define ReloadRxTimer4Lpl()	RxTimer_Stop();RxTimer_WritePeriod(LPL_SAMPLETIMING);ClearInterrupt(RxTimer_INT_CLR, RxTimer_INT_MASK);RxTimer_Start();  
#define ReloadRxTimer4Timeout()	RxTimer_Stop();RxTimer_WritePeriod(LPL_MAX_IDLE_TIMING);ClearInterrupt(RxTimer_INT_CLR, RxTimer_INT_MASK);RxTimer_Start();  
#define ReloadRxTimer4Backoff()	RxTimer_Stop();RxTimer_WritePeriod(LPL_WAKEUPTIMING);ClearInterrupt(RxTimer_INT_CLR, RxTimer_INT_MASK);RxTimer_Start(); 

// Packet to store receveived data in.
Packet tempPkt;

unsigned char dcRxStatus = 0;
unsigned char TxTimer = 12;

#define CRC_POLY	0x8408
unsigned int crc = 0;
extern const unsigned int CRCTable;
const unsigned int *crcTable = 0x0000;
unsigned int lastSrc, lastSeqNo;

//**********************************************
// Calculates CRC (using table lookup)
//**********************************************
void CalCRC(unsigned char *pkt, unsigned char length){
	unsigned char crccounter;
	// Move off length byte (length not included in crc)
	pkt++;
	//Calculate CRC
	crc = 0x0000;
	for (crccounter=1; crccounter<=length; crccounter++){
		crc = ((crc>>8)) ^ crcTable[ (unsigned char)crc ^ *pkt ];
		pkt++;
	}
}

//**********************************************
// Uses SPIS module for continuous data streaming
//**********************************************
BYTE SPIS_IO(BYTE in){
	while( !( SPIS_CONTROL_REG & SPIM_SPIM_TX_BUFFER_EMPTY ) );  
	SPIS_TX_BUFFER_REG = in;
	return SPIS_RX_BUFFER_REG;
}

//**********************************************
// Initialize the duty-cycle routines
//**********************************************
void SpeckMAC_Init(){
	unsigned char x;

	crcTable = &CRCTable;
	//configure the duty-cycling
	RxTimer_EnableInt();  
	ReloadRxTimer4Lpl();
	//Init receiver & enable interrupt for packets
	dcRxStatus=DC_RX_OFF;
	Radio_receiverOff;
	M8C_EnableIntMask(INT_MSK0,INT_MSK0_GPIO);
	M8C_EnableGInt;
	// Disable SFD Interrupt
	//SFD_IntCtrl_0_ADDR |= SFD_MASK;
	//SFD_IntCtrl_1_ADDR &= ~SFD_MASK;
	// Variables to help reduce number of the same packet that are processed
	lastSrc = 0xFFFF;
	lastSeqNo = 0xFFFF;
	
	#ifdef SPECKMAC_ONTIME_STATS
		OnTimeTimer_WritePeriod(ONTIME_TIMER_START_VALUE); // load timer with maximum value
	#endif
	
}

//**********************************************
// Stop the MAC, Turn off the radio
//**********************************************
void SpeckMAC_Stop(void){
	RxTimer_Stop();
	RxTimer_DisableInt();
	dcRxStatus=DC_RX_OFF;
	Radio_receiverOff;
	M8C_DisableIntMask(INT_MSK0,INT_MSK0_GPIO);	//disable recieve pkt interrupt
}

//**********************************************
// Restart the MAC,
//**********************************************
void SpeckMAC_Restart(void) {
	RxTimer_EnableInt();
	RxTimer_Start();
	Radio_receiverOff;
	dcRxStatus = DC_RX_OFF;
	M8C_EnableIntMask(INT_MSK0,INT_MSK0_GPIO);	//ensable recieve pkt interrupt
}


//****************************************************
// Handler for sampling the transmission duration
//****************************************************
#pragma interrupt_handler TxTimer_ISR
void TxTimer_ISR(){
	TxTimer = 0;
}

//****************************************************
// Handler for doing channel sampling
//****************************************************
#pragma interrupt_handler RxTimer_ISR
void RxTimer_ISR(){
	unsigned char x;
	
	#ifdef SPECKMAC_ONTIME_STATS	
		unsigned long rxOnTime = 0;
		if (stats_ctrl & STATS_RUN_PROGRAM) { // only count stats when in normal operation
			OnTimeTimer_WritePeriod(ONTIME_TIMER_START_VALUE);
			OnTimeTimer_Start();
		}
	#endif
	
	ReloadRxTimer4Lpl();   // Start listen timer
	if (dcRxStatus == DC_RX_OFF) 
		Radio_receiverOn;  // only turn on the receiver if it is currently off (if returning here after timeout (so == RX_DATA) then receiver is on.
	while (!(RADIO_SPIM_IO(0x00) & 0x02));  //loop until RSSI stable complete... at least 128us
	for (x=0;x<5;x++){
		if (!(CCA_Data_ADDR & CCA_MASK)){
			//Channel activity detected
			dcRxStatus = DC_RX_DATA; // posibly data on the channel so block tx
			ReloadRxTimer4Timeout();
			return;
		}
	}
	Radio_receiverOff;
	dcRxStatus = DC_RX_OFF;
	
	#ifdef SPECKMAC_ONTIME_STATS
		if (stats_ctrl & STATS_RUN_PROGRAM) { // only count stats when in normal operation
			OnTimeTimer_Stop();
			rxOnTime = (ONTIME_TIMER_START_VALUE - OnTimeTimer_wReadCounter());
			rxOntime += ( stats_readValue(STAT_RX_ON_HI) << 16 );
			rxOntime += stats_readValue(STAT_RX_ON_LO);
			stats_setValue(STAT_RX_ON_HI, rxOntime >> 16);
			stats_setValue(STAT_RX_ON_LO, rxOntime & 0x0000FFFF);			
		}
	#endif
	
}


//**********************************************
//Function to send pkt repetitively until duration is expired. Will send for time=duration and then extraSend more packets are transmitted
//NOTE!!! crc must be passed in as a global variable
//**********************************************
void SpeckMAC_serialTx(unsigned char *pkt, unsigned char length, unsigned int duration, unsigned char extraSend){
	unsigned char counter;	
	// Configure PSoC SPIS to use FIFOP as SCLK and FIFO as DATA 
	PRT3GS = 0x97;
	PRT3DM0 = 0x07;
	PRT3DM1 = 0xf8;
	PRT3IE = 0x00;
	PRT3IC1 = 0x00;
	// Start SPIS Module...
	SPIS_Start(SPIS_SPIS_MODE_0|SPIS_SPIS_LSB_FIRST);
	#ifdef SPECKMAC_ONTIME_STATS
		OnTimeTimer_WritePeriod(ONTIME_TIMER_START_VALUE); // load timer with maximum value
		OnTimeTimer_Start();
	#endif
	// Set CC2420 to serial TX mode
	RADIO_SPIM_OUT(0x12);	// MDMCTRL1
	RADIO_SPIM_OUT(0x00);
	RADIO_SPIM_OUT(0x04);	// Set mode to serial TX mode
	// Setup TxTimer here to determine when to end sending
	TxTimer_Stop();
	TxTimer_WritePeriod(duration);
	ClearInterrupt(TxTimer_INT_CLR, TxTimer_INT_MASK);
	TxTimer_DisableInt();
	// Start transmitting
	RADIO_SPIM_OUT(0x04); // Enable Transmitt
	TxTimer_Start(); // Start the tx timer.
	// Transmit the first packet
	SPIS_SetupTxData(length+2);	 // Add two for CRC (in global crc variable)
	SPIS_EnableSS();	// Set SPIS module Slave Select low  (SW Select Mode)
	for (counter=1; counter<=length; counter++){	// SPI the Data
		SPIS_IO(*(pkt+counter));
	}
	SPIS_IO( (unsigned char)* (((unsigned char*)&crc)+1) );	//CRC Lsb
	SPIS_IO( (unsigned char)* ((unsigned char*)&crc));	//CRC Msb
	// Transmit packets until Tx Timer expires
	while (!(TxTimer_INT_CLR & TxTimer_INT_MASK)){	
		SPIS_IO(0x00);
		SPIS_IO(0x00);
		SPIS_IO(0x00);
		SPIS_IO(0x00);
		SPIS_IO(0xA7);		//SFD
		SPIS_IO(length+2);	//Length Field
		for (counter=1; counter<=length; counter++){	//Data
		    while( !( SPIS_CONTROL_REG & SPIM_SPIM_TX_BUFFER_EMPTY ) );
			SPIS_IO(*(pkt+counter));
		}
		SPIS_IO( (unsigned char)* (((unsigned char*)&crc)+1) );	//CRC Lsb
		SPIS_IO( (unsigned char)* ((unsigned char*)&crc));	//CRC Msb
	}
	// Transmit any extra sends
	for(;extraSend>0;extraSend--){
		SPIS_IO(0x00);
		SPIS_IO(0x00);
		SPIS_IO(0x00);
		SPIS_IO(0x00);
		SPIS_IO(0xA7);		//SFD
		SPIS_IO(length+2);	//Length Field...
		for (counter=1; counter<=length; counter++) {	//Data
   			SPIS_IO(*(pkt+counter));
   		}
		SPIS_IO( (unsigned char)* (((unsigned char*)&crc)+1) );	//CRC Lsb
		SPIS_IO( (unsigned char)* ((unsigned char*)&crc));	//CRC Msb
	}

	SPIS_IO(0x00);		//Tx the last byte
	Radio_receiverOff; 	// MAke sure receiver is off
	RADIO_SPIM_OUT(0x08);	// Flush the Radio buffers
	RADIO_SPIM_OUT(0x08);
	RADIO_SPIM_OUT(0x09);
	RADIO_SPIM_OUT(0x09);
	#ifdef SPECKMAC_ONTIME_STATS
		OnTimeTimer_Stop();						
		stats_incValue(STAT_TX_ON, ONTIME_TIMER_START_VALUE - (uint16_t) OnTimeTimer_wReadCounter());
	#endif
	//Set PSoC Radio pins back to Normal Mode
	PRT3GS = 0x95;
	PRT3DM0 = 0x05;
	PRT3DM1 = 0xf9;
	PRT3IE = 0x80;
	PRT3IC1 = 0x80;
	// Turn off SPIS Module
	SPIS_DisableSS();
	SPIS_Stop();
	// Set radio back to buffered modes.
	RADIO_SPIM_IO(0x12);	//MDMCTRL1
	RADIO_SPIM_IO(0x00);	//set to 0x05 for hys 20
	RADIO_SPIM_IO(0x00);	//Reset back to normal mode
	// Stop the tx-timer
	TxTimer_Stop();
	TxTimer_DisableInt();
	ClearInterrupt(TxTimer_INT_CLR, TxTimer_INT_MASK);
}

//****************************************
// Sends Data packet as the wakeup signal
// Used by SpeckMAC_Data
//****************************************
unsigned char SpeckMAC_SendDataWakeup(BYTE * pkt){
	unsigned char cpuf;
	cpuf = CPU_F;
	M8C_DisableGInt;	
	Radio_receiverOn;	
	while (!(RADIO_SPIM_IO(0x00) & 0x02));  //loop until RSSI stable complete, (RF is then stable)
	// Sample the channel for a duration defined by "LPL_CSMA_CHANNEL_SAMPLE_DURATION" in lpl.h
 	TxTimer_Stop();
	TxTimer_DisableInt();
	ClearInterrupt(TxTimer_INT_CLR, TxTimer_INT_MASK);
	TxTimer_WritePeriod(LPL_CSMA_SAMPLE_DURATION_CLK);
	TxTimer_Start();
	while (!(TxTimer_INT_CLR & TxTimer_INT_MASK)){	
		if (!(CCA_Data_ADDR & CCA_MASK)){
			dcRxStatus = DC_RX_DATA;
			ReloadRxTimer4Timeout();
			M8C_EnableGInt;
			#ifdef SPECKMAC_DEBUG
				UART_CPutString("Channel Busy \r\n"); Radio_delay(1);
			#endif
			return FAILURE;
		}
	}
	ClearInterrupt(TxTimer_INT_CLR, TxTimer_INT_MASK);
	TxTimer_Stop();
	Radio_receiverOff;	
	SpeckMAC_serialTx(pkt, (*((char*)pkt))+MIN_PKT_HDR_LEN, LPL_WAKEUPTIMING, 1);
	// If interrupts were previously enabled then re-enable.
	if (cpuf&0x01)
		M8C_EnableGInt;
	return SUCCESS;
}

//****************************************************
//IMPT!!! THIS FUNCTION SHOULD NEVER BE A PART OF A INTERRUPT SERVICE ROUTINE
//Sends packet and return SUCCESS if packet can be sent, FAILURE otherwise
//****************************************************
unsigned char SpeckMAC_SendPkt(Packet *pkt){
	RxTimer_DisableInt();
	if (dcRxStatus != DC_RX_OFF){	//ProSpeckz in receiver mode... expecting packet
		RxTimer_EnableInt();
		return FAILURE; 
	}
	RxTimer_Stop();		//since next function will either send packet or set ProSpeckz to receive mode, there is no more need for the RxTimer	
	ClearInterrupt(RxTimer_INT_CLR, RxTimer_INT_MASK);
	RxTimer_EnableInt();
	//Channel free, clear to transmit
	if (SpeckMAC_SendDataWakeup((BYTE*) pkt)==FAILURE) {
		return FAILURE;
	}
	Radio_receiverOff;	
	dcRxStatus = DC_RX_OFF;
	ReloadRxTimer4Lpl(); // MAC is in active mode so restart receiver dutycycling
	return SUCCESS;
}

//*********************************************************
//IMPT!!! THIS FUNCTION SHOULD NEVER BE A PART OF A INTERRUPT SERVICE ROUTINE
//Blocks the processor until packet is sent
//*********************************************************
void SpeckMAC_SendPktBlocking(Packet *pkt){
	unsigned long backoffTime;
	CalCRC((BYTE *)pkt, (*((unsigned char*)pkt))+MIN_PKT_HDR_LEN);	//CRC for packet is stored as a global variable
	//some internal error on the compiler prevents passing crc as a variable using RADIO_BlockSend (alledgedly)
	pkt->rssi = (unsigned char)* (((unsigned char*)&crc)+1); // the msb of the crc
	pkt->crc = (unsigned char)* ((unsigned char*)&crc); // crc lsb.
	while (1){
		while (dcRxStatus != DC_RX_OFF){ 
			// Only way that this occurs is if the rx timer interrupt has just detected busy channel so data is about to be received...
			// Sleep here: Will be woken up by either rxtimer or on data reception interrupt, main program execution will sit here untill channel is free
			GoToSleep;
		}	
		// Now send the packet.	
		if (SpeckMAC_SendPkt(pkt)==SUCCESS) {			
			Radio_delay(1);
			break;	
		}
		backoffTime = (unsigned long) (LPL_WAKEUPTIMING + ((GetRandomInt()%1666)-LPL_WAKEUPTIMING));
		#ifdef SPECKMAC_DEBUG
		UART_CPutString("Backoff: 0x"); UART_PutSHexInt(backoffTime >>16); UART_PutSHexInt(backoffTime); UART_CPutString("\r\n"); Radio_delay(1);
		#endif
		// If unsuccessful, delay a bit then try again...
		// Upto 50 ms delay before trying again.
	 	TxTimer_Stop();
		TxTimer_WritePeriod(backoffTime);	//0..50ms delay
		TxTimer = 1;
		TxTimer_EnableInt();
		TxTimer_Start();
		while (TxTimer==1){
			// Sleep untill the txtimer interrupt fires
			GoToSleep;
		}
		TxTimer_Stop();
		TxTimer_DisableInt();
	}
}


//****************************************************
// Interrupt handler for receiving radio packets from CC2420
//****************************************************
#pragma interrupt_handler PSoC_GPIO_ISR
void PSoC_GPIO_ISR(){
	//#ifdef PRINT_ALL_RX
	unsigned char x;
	//#endif
	unsigned char pktRx = FALSE;
	receiveStatus_t status;
	// Find the cause of the interrupt. 		
	// If FIFOP pin is high there is a complete packet or the buffer has overflowed...
	if (FIFOP_Data_ADDR & FIFOP_MASK) {
		UART_CPutString("RX"); Radio_delay(10);
		// Complete packet in the buffer (or Buffer past threshold, using 127 for threshold... ie buffer is full)
		Radio_receiverOff;
		dcRxStatus = DC_RX_OFF;		
		// If FIFO pin is dropped then the buffer is overflowed...
		if( ! (FIFO_Data_ADDR & FIFO_MASK ) ) {
			// Buffer is full of crap.
			//UART_CPutString("RX Buffer Overflow\r\n"); Radio_delay(1);
			/*
			 * Send SFLSHRX Strobe twice to recover SFD Pin (CC2420 Datasheet, pg 33);
			 * Read a byte from the buffer first to make sure the buffer access is possible. (CC2420 Datasheet, pg 62);
			 */
			CSn_Data_ADDR |= CSn_MASK;			// Disable CC2420 SPI 
			CSn_Data_ADDR &= ~CSn_MASK;			// Enable CC2420 SPI
			RADIO_SPIM_IO(0x40 | 0x3F);			//read a byte from RX FIFO Buffer
			RADIO_SPIM_IO(0x00);
			CSn_Data_ADDR |= CSn_MASK;			// Disable CC2420 SPI 
			CSn_Data_ADDR &= ~CSn_MASK;			// Enable CC2420 SPI
			// Now clear it...
			Radio_rxFlush;
			Radio_rxFlush;	
		}	
		else {
			while( (status = Radio_receive( (unsigned char *)&tempPkt, sizeof(Packet) )).radioStatus == SUCCESS ) {	
				// Radio receive turns receiver back on, might want to turn it off again... 
				Radio_receiverOff;
				pktRx = TRUE;
				if( tempPkt.dataLength < (MIN_PKT_HDR_LEN + 2) ) {
					// This is bollocks, length is not enough to be a valid packet, slightly strange to get here...
					//UART_CPutString("RX Length < HDRs\r\n"); Radio_delay(1);
				}
				else if ( tempPkt.dataLength <= sizeof(Packet) ) {	//handle "data" packet pointed by tempPkt		
					#ifdef ENABLE_SPECKMAC_SEQUENCE_NUMBER_FILTERING
					if(tempPkt.srcAddr == lastSrc && tempPkt.seqNo == lastSeqNo)  {
						// Is the same as the previous packet
						#ifdef SPECKMAC_DEBUG
							UART_CPutString("DISCARD"); Radio_delay(10);
						#endif
					}
					else {
					#endif
						// Radio_receive in prospeckz_radio.c  (bootloader library) returns length of all bytes returned
						tempPkt.dataLength -= (MIN_PKT_HDR_LEN + 2);
						// Copy RSSI and LQI from receive status into packet
						tempPkt.rssi = status.rssi;
						tempPkt.crc = status.lqi;
						// Holders for fields of last packet ( to prevent doing the same thing over and over if a packet is received multiple times)
						lastSrc = tempPkt.srcAddr;
						lastSeqNo = tempPkt.seqNo;	
						//Data packet received, handle and return to lpl
						#ifdef PRINT_ALL_RX	
								UART_CPutString("RX :: Src: 0x"); UART_PutSHexInt(tempPkt.srcAddr);
								UART_CPutString(" Dest: 0x"); UART_PutSHexInt(tempPkt.destAddr);
								UART_CPutString(" Length: 0x");	UART_PutSHexByte(tempPkt.dataLength);	
								UART_CPutString(" RT: 0x");	UART_PutSHexInt(tempPkt.routingAddr);
								UART_CPutString(" SeqNo: 0x");	UART_PutSHexInt(tempPkt.seqNo);
								UART_CPutString(" CRC: 0x");	UART_PutSHexByte(tempPkt.crc);
								UART_PutCRLF();			
								if (tempPkt.dataLength > 0) {
									x = 0;
									UART_CPutString("Data: ");				
									while(x < tempPkt.dataLength) {
										UART_PutSHexByte(tempPkt.data[x]);
										UART_CPutString(" : ");
										++x;
									}
									UART_PutCRLF();
								}									
						#endif
						#ifdef ENABLE_STATISTICS
						// if this is a stats message then go directly to the stats handler
						if( tempPkt.routingAddr == STATS_ADDR && tempPkt.data[0] == PACKET_STATISTICS ) {						
//							UART_CPutString("RX :: Src: 0x"); UART_PutSHexInt(tempPkt.srcAddr);
//							UART_CPutString(" Dest: 0x"); UART_PutSHexInt(tempPkt.destAddr);
//							UART_CPutString(" Length: 0x");	UART_PutSHexByte(tempPkt.dataLength);	
//							UART_CPutString(" RT: 0x");	UART_PutSHexInt(tempPkt.routingAddr);
//							UART_CPutString(" SeqNo: 0x");	UART_PutSHexInt(tempPkt.seqNo);
//							UART_CPutString(" CRC: 0x");	UART_PutSHexByte(tempPkt.crc);
//							UART_PutCRLF();			
//							if (tempPkt.dataLength > 0) {
//								x = 0;
//								UART_CPutString("Data: ");				
//								while(x < tempPkt.dataLength) {
//									UART_PutSHexByte(tempPkt.data[x]);
//									UART_CPutString(" : ");
//									++x;
//								}
//								UART_PutCRLF();
//							}												
							if(tempPkt.destAddr == routing_myAddr || tempPkt.destAddr == BROADCAST_ADDR) {				
								stats_handlePacket(&tempPkt);
							}
						}
						#endif						
						#ifdef ENABLE_STATISTICS							
							else if (stats_ctrl & STATS_RUN_PROGRAM) {
						#endif
								#ifdef ENABLE_ROUTING	
									// Pass the packet on to routing layer...	
									routing_handlePacket(&tempPkt);
								#endif
						#ifdef ENABLE_STATISTICS							
							}
						#endif	
					#ifdef ENABLE_SPECKMAC_SEQUENCE_NUMBER_FILTERING								
					}				
					#endif
				}
				else {
					//UART_CPutString("RX Length > Pkt\r\n"); Radio_delay(1);
				}
			}
			// Either buffer is empty or radio_receive returned a fail.
			CSn_Data_ADDR |= CSn_MASK;			// Disable CC2420 SPI 
			CSn_Data_ADDR &= ~CSn_MASK;			// Enable CC2420 SPI
			RADIO_SPIM_IO(0x40 | 0x3F);			//read a byte from RX FIFO Buffer
			RADIO_SPIM_IO(0x00);
			CSn_Data_ADDR |= CSn_MASK;			// Disable CC2420 SPI 
			CSn_Data_ADDR &= ~CSn_MASK;			// Enable CC2420 SPI
			// Now clear it...
			Radio_rxFlush;
			Radio_rxFlush;	
		}	
	}
	else {
		//UART_CPutString(" : E\r\n"); Radio_delay(1);	
	}
	ReloadRxTimer4Lpl();			
}
